import rclpy
from rclpy.node import Node
import serial
import time
from geometry_msgs.msg import Twist
import math
import struct

class rxFrame:
    def __init__(self):
        self.rawBytes = []
        self.frame = {
            "dataLen": 0,
            "sc1": 0,
            "sc2": 0
        }

class Uart32(Node):
    def __init__(self):
        super().__init__("uart_to_32")
        self.ser = serial.Serial('/dev/ttyS9', 115200)
        self.sub_cmd_vel = self.create_subscription(Twist, "cmd_vel", self.cmd_vel_callback, 10)
        self.timer = self.create_timer(0.001, self.timer_callback)



        # Instantiate the rxFrame object
        self.rxFrame = rxFrame()

        # State machine related variables
        self.recv_cnt = 0
        self.data_cnt = 0
        self.rxstate = 0

    def timer_callback(self):
        #读取串口
        self.get_serial()
        pass



    def cmd_vel_callback(self, msg):
        self.get_logger().info(f"x:{msg.linear.x}, y:{msg.linear.y},z:{msg.linear.z}, yaw:{msg.angular.z}")


    def get_serial(self):
        _cnt = self.ser.in_waiting
        if _cnt != 0:
            self.read_buf = self.ser.read(_cnt)
            for byte in self.read_buf:
                print(hex(byte))
                if self.res_oneByte(byte):
                    return True
        return False


    def res_oneByte(self, data):
        print(self.rxstate)
        # Check if the frame header is 0xAA as per the anonymous protocol
        if self.rxstate == 0 and data == 0xAA:
            self.recv_cnt = 0
            self.rxFrame.rawBytes.append(data)
            self.recv_cnt += 1
            self.rxstate += 1
        # Data address byte
        elif self.rxstate == 1:
            self.rxFrame.rawBytes.append(data)
            self.recv_cnt += 1
            self.rxstate += 1
        # Frame ID
        elif self.rxstate == 2:
            self.rxFrame.rawBytes.append(data)
            self.recv_cnt += 1
            self.rxstate += 1
        # Receive data length
        elif self.rxstate == 3:
            self.rxFrame.rawBytes.append(data)
            self.recv_cnt += 1
            self.rxFrame.frame["dataLen"] = data
            self.data_cnt = 0
            self.rxstate += 1
        # Receive data
        elif self.rxstate == 4:
            self.rxFrame.rawBytes.append(data)
            self.recv_cnt += 1
            self.data_cnt += 1
            if self.data_cnt >= self.rxFrame.frame["dataLen"]:
                self.rxstate += 1
        # Receive SC1
        elif self.rxstate == 5:
            self.rxFrame.frame["sc1"] = data
            self.rxstate += 1
        # Receive SC2
        elif self.rxstate == 6:
            self.rxFrame.frame["sc2"] = data
            self.rxstate = 0
            # All data has been received, perform parsing
            return self.res_anl(self.rxFrame.rawBytes)

            
        else:
            self.rxstate = 0
            return False

        return False

    def res_anl(self, data):
        check_sum1 = 0
        check_sum2 = 0

        # Calculate checksum1 and checksum2 based on received data
        for i in range(self.rxFrame.frame["dataLen"] + 4):
            check_sum1 += self.rxFrame.rawBytes[i]
            check_sum2 += check_sum1

        # Compare calculated checksums with received checksums
        if check_sum1 != self.rxFrame.frame["sc1"] or check_sum2 != self.rxFrame.frame["sc2"]:
            return False

        # Process data based on frame ID
        frame_id = self.rxFrame.rawBytes[2]

        if frame_id == 0x05:
            # Altitude data
            self.anoMsgAlt.Alt = struct.unpack_from('<h', self.rxFrame.rawBytes, 4)[0] / 100.0
            print(self.anoMsgAlt.Alt)
            return True
        elif frame_id == 0xF2:
            # Orangepi control data
            self.Orangepi_Control.s161 = struct.unpack_from('<h', self.rxFrame.rawBytes, 4)[0]
            return True
        else:
            return False

def main(args=None):
    rclpy.init(args=args)
    node = Uart32()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()
